classdef analyseATRIAS < atrias
    %Methods to apply to atrias exported data
    %2013-02-15
    
    properties
        SLIP;
        Energetics;
    end
    
    methods
        function obj=analyseATRIAS() %Constructor
            obj.Version.class='analyseATRIAS';
        end
        
        function obj = mech_energy(obj)
            obj.Energetics.Epot = obj.robotProperties.m * 9.81 * obj.Kinematics.position(:,2);
            obj.Energetics.Ekin = 1/2 * obj.robotProperties.m * (obj.Kinematics.velocity(:,1) + obj.Kinematics.velocity(:,2));
            obj.Energetics.EspringRot = 1/2 * obj.robotProperties.krot * obj.Kinematics.springDeflection.^2;
            F_filt = -obj.Dynamics.axLegForce.*(obj.Dynamics.axLegForce<-80);
            %l_filt = l_filt .* (l_filt <= 0.9);
            obj.Energetics.EspringLeg = 3.3e-8.*F_filt.^3 + 2e-5.*F_filt.^2 + 0.0022 .*F_filt - 0.084; 
        end
        
        function obj = fit_SLIP(obj)
           %addpath C:\Users\Work\Documents\Lausanne\Repositories\spring-mass-model
           M = nlSLIP;
           M.params.m = 60;
           M.params.krot = 1600;
           M.params.l0 = 0.9;
           M.params.aTD = 1.144;
           M.state.IC = [0 0.763 0.87 0];
           M.state.xe = M.state.IC;
           M = int_walk_single(M);
           M = int_walk_double(M);
           M = int_walk_single(M);
           M = int_walk_single(M);
           M = int_walk_double(M);
           obj.SLIP.tstart = M.state.FP(2,3);
           obj.SLIP.tend = M.state.FP(3,3);
           M.state.FP = M.state.FP(1:2,1:3);
           M = data_processing(M);
           obj.SLIP.F = M.data.F(2,M.data.F(2,:)>0)';
           obj.SLIP.x = M.state.x(M.data.F(2,:)>0,:);
           obj.SLIP.t = M.state.t(M.data.F(2,:)>0);
           obj.SLIP.Fx = M.data.Fx(2,M.data.F(2,:)>0)';
           obj.SLIP.Fy = M.data.Fy(2,M.data.F(2,:)>0)';
           obj.SLIP.E = M.data.E;
           obj.SLIP.tnorm = (obj.SLIP.t-obj.SLIP.t(1)) ./ (obj.SLIP.t(end)-obj.SLIP.t(1)) .* 1000;
           [~,obj.SLIP.apex] = max(M.state.x(:,3));
           obj.SLIP.RM = [M.state.x(1,:); M.state.x(obj.SLIP.apex,:)];
        end
        
        function obj=mechCOT(obj)
            % calculate collision angle
            
            obj = COT(obj,0);
            
            obj.COT_Data.collang=asin(abs(dot([obj.Dynamics.Fx(:,3) obj.Dynamics.Fy(:,3)]',obj.Kinematics.velocity(:,[1 2])'))'./sqrt(obj.Kinematics.velocity(:,1).^2+obj.Kinematics.velocity(:,2).^2)./sqrt(obj.Dynamics.Fx(:,3).^2+obj.Dynamics.Fy(:,3).^2));
            
            % CoMoT
            FV=abs(dot([obj.Dynamics.Fx(:,3) obj.Dynamics.Fy(:,3)]',obj.Kinematics.velocity(:,[1 2])'));
            absFV=(sqrt(obj.Kinematics.velocity(:,1).^2+obj.Kinematics.velocity(:,2).^2).*sqrt(obj.Dynamics.Fx(:,3).^2+obj.Dynamics.Fy(:,3).^2))';
            obj.COT_Data.CoMoT=NaN(length(FV),1);
            obj.COT_Data.CoMoT(FV==FV)=cumsum(FV(FV==FV))./cumsum(absFV(absFV==absFV));
            
            % COT
            % select stride
            if isfield(obj.Timing,'stanceRight')
                for idx=1:length(obj.Timing.stanceRight(:,1))-1
                    stridestart=obj.Timing.stanceRight(idx);
                    strideend=obj.Timing.stanceRight(idx+1)-1;
                    samples=sum(~isnan(obj.Kinematics.velocity(stridestart:strideend,2)));  %number of valid samples
                    absFVstride=nansum(abs(dot([obj.Dynamics.Fx(stridestart:strideend,3) obj.Dynamics.Fy(stridestart:strideend,3)]',obj.Kinematics.velocity(stridestart:strideend,[1 2])')));
                    obj.COT_Data.absVy(idx)=nanmean(abs(obj.Kinematics.velocity(stridestart:strideend,2)));
                    obj.COT_Data.mechCOTcollang(idx)=absFVstride./samples./obj.COT_Data.absVy(idx)./obj.robotProperties.m/obj.robotProperties.g;
                    obj.COT_Data.mechPower(idx)=nansum(abs(dot([obj.Dynamics.Fx(stridestart:strideend,3) obj.Dynamics.Fy(stridestart:strideend,3)]',obj.Kinematics.velocity(stridestart:strideend,[1 2])'))./samples);
                    obj.COT_Data.mechCOT(idx)=obj.COT_Data.mechPower(idx)/obj.robotProperties.m/obj.robotProperties.g/obj.COT_Data.VelocitySmoothMean;
                end
            end
            samples=sum(~isnan(obj.Kinematics.velocity(:,2)));  %number of valid samples
            obj.COT_Data.mPower=nansum(abs(dot([obj.Dynamics.Fx(:,3) obj.Dynamics.Fy(:,3)]',obj.Kinematics.velocity(:,[1 2])'))./samples);
            obj.COT_Data.mCOT=obj.COT_Data.mPower/obj.robotProperties.m/obj.robotProperties.g/obj.COT_Data.VelocitySmoothMean;
            
        end
        
        function obj = COT(obj,fig)
            VelocitySmooth=NaN(length(obj.Kinematics.velocity),1);
            try
                VelocitySmooth(obj.Kinematics.velocity(:,1)==obj.Kinematics.velocity(:,1)) = SPoleFilt(obj.Kinematics.velocity(obj.Kinematics.velocity(:,1)==obj.Kinematics.velocity(:,1),1), exp(-2*pi*obj.Analytics.FrequencyCutOff / obj.Analytics.SamplingRate), 2,obj.Analytics.type);
            catch
                VelocitySmooth=obj.Kinematics.velocity(:,1);
            end
            obj.COT_Data.VelocitySmoothMean = nanmean(VelocitySmooth);
            
            % Froude number calcuation (Alexander, 1987)
            obj.COT_Data.FROUDE = obj.COT_Data.VelocitySmoothMean^2/(obj.robotProperties.g * obj.robotProperties.l0);
            
            %% Mean the Low pass filter the motor coil voltage, instantaneous, per motor
            VoltageInstantSmooth=NaN(size(obj.Electrics.motorVoltage));
            for idx=1:4
                try
                    VoltageInstantSmooth(obj.Electrics.motorVoltage(:,idx)==obj.Electrics.motorVoltage(:,idx),idx) = SPoleFilt(obj.Electrics.motorVoltage(obj.Electrics.motorVoltage(:,idx)==obj.Electrics.motorVoltage(:,idx),idx), exp(-2*pi*obj.Analytics.FrequencyCutOff / obj.Analytics.SamplingRate), 2,obj.Analytics.type);
                catch
                    VoltageInstantSmooth=obj.Electrics.motorVoltage;
                end
                obj.COT_Data.VoltageSmoothMean(idx) = nanmean(VoltageInstantSmooth(:,idx));
            end
            obj.COT_Data.Current50mean=mean(obj.Electrics.measuredCurrent(:,1));
            % asp, adding the total current
            obj.COT_Data.Current50meanTotal=mean((obj.Electrics.measuredCurrent(:,1)+obj.Electrics.measuredCurrent(:,2)));
            obj.COT_Data.VoltageMean=nanmean(obj.COT_Data.VoltageSmoothMean);
            obj.COT_Data.PowerClamp50=obj.COT_Data.Current50mean*obj.COT_Data.VoltageMean;
            % asp, adding the total power
            obj.COT_Data.PowerClamp50Total=obj.COT_Data.Current50meanTotal*obj.COT_Data.VoltageMean;
            obj.COT_Data.CotClamp50 = obj.COT_Data.PowerClamp50 / (obj.robotProperties.m * obj.robotProperties.g * obj.COT_Data.VelocitySmoothMean);
            % asp, adding the COT of total
            obj.COT_Data.CotClamp50Total = obj.COT_Data.PowerClamp50Total / (obj.robotProperties.m * obj.robotProperties.g * obj.COT_Data.VelocitySmoothMean);
            if fig
                figure();
                axes1 = axes('FontSize',16);
                try
                    plot1 = plot([...
                        obj.Electrics.measuredCurrent(:,1)...
                        (SPoleFilt(obj.Electrics.measuredCurrent(:,1), ...
                        exp(-2*pi*obj.Analytics.FrequencyCutOff / ...
                        obj.Analytics.SamplingRate), 2,obj.Analytics.type)) ...
                        obj.Electrics.measuredCurrent(:,2)...
                        ]);
                    set(plot1(1),'Color',[0 0 0],...
                        'DisplayName','Current consumption intermediate',...
                        'LineStyle','-','LineWidth',1);
                    set(plot1(2),'Color',[1 0 0],...
                        'DisplayName','Current consumption low pass filered',...
                        'LineStyle','-','LineWidth',2);
                    set(plot1(3),'Color','blue',...
                        'DisplayName','Current negative',...
                        'LineStyle','-','LineWidth',2);
                catch
                    
                    plot1 = plot(obj.Electrics.measuredCurrent(:,1));
                    set(plot1(1),'Color',[0 0 0],...
                        'DisplayName','Current consumption intermediate',...
                        'LineStyle','-','LineWidth',1);
                end
                
                xlabel('Time in [ms]');
                ylabel('Current in [A]');
                legend1 = legend(axes1,'show');
                set(legend1,'Location','North');
                % axis([5000 9000 -5 30]);
                % saveas(figure1, 'Time_Current_0p3mpsRun.eps', 'eps2c')
            end
        end
        
        function plot_F_global(obj,number)
            figure();
            axes1 = axes('FontSize',16);
            %
            %             obj = calcDynamics(obj,forcefile);
            %             obj=mechCOT(obj);
            
            if number==1
                plot1 = plot([...
                    obj.Dynamics.Fx(:,1)...
                    obj.Dynamics.Fy(:,1)...
                    ]);
                set(plot1(1),...
                    'DisplayName','F_{hori}_{ LL}',...
                    'LineStyle','-','LineWidth',1);
                set(plot1(2),...
                    'DisplayName','F_{vert}_{ LL}',...
                    'LineStyle','-','LineWidth',1);
                if ~isfield(obj.Dynamics.Fx,'FP')
                    plot(obj.Dynamics.FP.Fx);
                end
            elseif number==2
                plot1 = plot([...
                    obj.Dynamics.Fx(:,2)...
                    obj.Dynamics.Fy(:,2)...
                    ]);
                set(plot1(1),...
                    'DisplayName','F_{hori}_{ RL}',...
                    'LineStyle','-','LineWidth',1);
                set(plot1(2),...
                    'DisplayName','F_{vert}_{ RL}',...
                    'LineStyle','-','LineWidth',1);
            else
                plot1 = plot([...
                    obj.Dynamics.Fx(:,1)...
                    obj.Dynamics.Fx(:,2)...
                    obj.Dynamics.Fy(:,1)...
                    obj.Dynamics.Fy(:,2)...
                    ]);
                set(plot1(1),...
                    'DisplayName','F_{hori}_{ LL}',...
                    'LineStyle','-','LineWidth',1);
                set(plot1(2),...
                    'DisplayName','F_{hori}_{ RL}',...
                    'LineStyle','-','LineWidth',1);
                set(plot1(3),...
                    'DisplayName','F_{vert}_{ LL}',...
                    'LineStyle','-','LineWidth',1);
                set(plot1(4),...
                    'DisplayName','F_{vert}_{ RL}',...
                    'LineStyle','-','LineWidth',1);
            end
            xlabel('Time in [ms]');
            ylabel('Force in [N]');
            legend1 = legend(axes1,'show');
            set(legend1,'Location','NorthEast');
        end
        
        function v_T_plot(obj)
            clf
            a=1;
            %a=7184;
            b=length(obj.Kinematics.motors.MotorVelocity);
            %b=8148;
            v=-8:0.01:8;
            I=500*23.3/193./v;
            quad=[1 3 2 4];
            for idx=1:4
                subplot(2,2,quad(idx))
                plot(obj.Kinematics.motors.MotorVelocity(a:b,idx),obj.Electrics.motorCurrent(a:b,idx))
                hold on
                plot(v,I,'m--')
                plot([0 -7.86],[-60 0],'--','Color',[.3 .3 .3])
                plot([0 7.86],[60 0],'--','Color',[.3 .3 .3])
                plot([0 -7.86],[-193 0],'k--')
                plot([0 7.86],[193 0],'k--')
                xlim([-8,8])
                ylim([-193 193]);
                grid on
            end
        end
        
        function toe_trajectory(obj)
            clf
            for idx=1:2
                subplot(2,1,idx)
                hold on
                plot(obj.Kinematics.footpoint.x(idx,:),obj.Kinematics.footpoint.y(idx,:));
                for jdx=2:size(obj.Timing.phaseShift,1)-1
                    %plot(obj.Kinematics.footpoint.x(idx,obj.Timing.phaseShift(jdx):obj.Timing.phaseShift(jdx+1)),obj.Kinematics.footpoint.y(idx,obj.Timing.phaseShift(jdx):obj.Timing.phaseShift(jdx+1)),'Color',[1-obj.Timing.phaseShift(jdx,2) obj.Timing.phaseShift(jdx,2) 1])
                    plot(obj.Kinematics.footpoint.x(idx,obj.Timing.phaseShift(jdx)),obj.Kinematics.footpoint.y(idx,obj.Timing.phaseShift(jdx)),'rx','Markersize',12)
                end
                xlim([min(obj.Kinematics.footpoint.x(idx,:)) max(obj.Kinematics.footpoint.x(idx,:))])
                if idx==2
                    text(-0.2,-0.88,'I','Fontsize',16)
                    text(-0.1,-0.75,'II','Fontsize',16)
                    text(0.3,-0.8,'III','Fontsize',16)
                    text(0.25,-0.87,'IV','Fontsize',16)
                    text(0.1,-0.84,'V','Fontsize',16)
                    text(-0.1,-0.84,'VI','Fontsize',16)
                else
                    text(-0.2,-0.88,'VI','Fontsize',16)
                    text(-0.1,-0.75,'V','Fontsize',16)
                    text(0.3,-0.8,'VI','Fontsize',16)
                    text(0.25,-0.87,'I','Fontsize',16)
                    text(0.1,-0.84,'II','Fontsize',16)
                    text(-0.1,-0.84,'III','Fontsize',16)
                end
                g(idx)=gca;
                pos=get(gca,'Position');
                a=ylim;
                b=xlim;
                h(idx)=axes('Position',pos);
                set(gca,'xlim',b,'ylim',a);
                linkaxes([g(idx) h(idx)],'xy');
                set(h(idx),'Color','none')
                uistack(h(idx),'top')
            end
            
            %             for idx=1:10:size(obj.Kinematics.footpoint.x,2)
            %                 axes(h(1));
            %                 cla
            %                 plot(obj.Kinematics.footpoint.x(1,idx),obj.Kinematics.footpoint.y(1,idx),'o','Markersize',10,'Linewidth',2)
            %                 set(gca,'xlim',b,'ylim',a);
            %                 set(gca,'Color','none');
            %                 axes(h(2));
            %                 cla
            %                 plot(obj.Kinematics.footpoint.x(2,idx),obj.Kinematics.footpoint.y(2,idx),'ro','Markersize',10,'Linewidth',2)
            %                 set(gca,'xlim',b,'ylim',a);
            %                 set(gca,'Color','none');
            %                 drawnow
            %             end
        end

        function toe_trajectory_ATCSlipWalking(obj)
            clf
            % A plot for each leg.  1 == left, 2 == right.
            for idx=1:2
                subplot(2,1,idx)
                hold on

                % Actual foot trajectory
                plot(obj.Kinematics.footpoint.x(idx,:),obj.Kinematics.footpoint.y(idx,:));

                % Ideal foot trajectory
                % Gait parameters
                r0 = 0.9;
                rRet = 0.15;
                % TODO: This is an eyeballed approximation.  Fixme
                qExtra = 0.04;
                q1 = 1.144+qExtra;
                q2 = 1.297;
                q3 = 1.845;
                q4 = (pi-q1)-qExtra;
                % Fake a stance phase
                qSl = linspace(q2,q3,50);
                % The nondimensional robot position
                s = (qSl - q2)/(q3 - q2);
                ds = ones(1,length(qSl))*1.5;
                % Trajectory
                for n = 1:length(s)
                    % Leg angle
                    [qm(n) dqm(n)] = cubicInterp(0, 0.90, q4, q1, -0.5, 1.0, s(n), ds(n));
                    % Leg length
                    if s(n) < 0.5
                        [rm(n) drm(n)] = cubicInterp(0, 0.5, r0, r0-rRet, -1.0, 0, s(n), ds(n));
                    else
                        [rm(n) drm(n)] = cubicInterp(0.5, 0.9, r0-rRet, r0, 0, 0, s(n), ds(n));
                    end
                end
                % Translate distances and angles to x and y
                x = rm.*cos(qm);
                y = -rm.*sin(qm);

                % Are we plotting the left or right leg?
                if idx == 1
                    touchdowns = obj.Timing.ltd;
                    takeoffs   = obj.Timing.lto;
                else
                    touchdowns = obj.Timing.rtd;
                    takeoffs   = obj.Timing.rto;
                end
                % Correct for takeoff position
                x = x - x(1) + mean(obj.Kinematics.footpoint.x(idx,takeoffs));
                y = y - y(1) + mean(obj.Kinematics.footpoint.y(idx,takeoffs));
                % Plot the toe trajectory
                plot(x,y,'g.')


                % Plot each event
                for n = touchdowns
                    plot(obj.Kinematics.footpoint.x(idx,n),obj.Kinematics.footpoint.y(idx,n),'rx','Markersize',12)
                end
                for n = takeoffs
                    plot(obj.Kinematics.footpoint.x(idx,n),obj.Kinematics.footpoint.y(idx,n),'kx','Markersize',12)
                end

                if idx == 1
                    legend('Left Foot Trajectory','Target Trajectory','Touchdown','Takeoff','Location','Best')
                else
                    legend('Right Foot Trajectory','Target Trajectory','Touchdown','Takeoff','Location','Best')
                end

                xlim([min(obj.Kinematics.footpoint.x(idx,:)) max(obj.Kinematics.footpoint.x(idx,:))])

                %{
                g(idx)=gca;
                pos=get(gca,'Position');
                a=ylim;
                b=xlim;
                h(idx)=axes('Position',pos);
                set(gca,'xlim',b,'ylim',a);
                linkaxes([g(idx) h(idx)],'xy');
                set(h(idx),'Color','none')
                uistack(h(idx),'top')
                %}
            end
        end

        function toe_mean_trajectory(obj)
            clf
            ax=[];ay=[];
            for idx=1:2
                %plot(obj.Kinematics.footpoint.x(idx,:),obj.Kinematics.footpoint.y(idx,:));
                if idx==1
                    for jdx=2:size(obj.Timing.phaseShift,1)-1
                        tx(mod(jdx,6)+(mod(jdx,6)==0)*6,floor((jdx-1)/6)+1)=obj.Kinematics.footpoint.x(idx,obj.Timing.phaseShift(jdx));
                        ty(mod(jdx,6)+(mod(jdx,6)==0)*6,floor((jdx-1)/6)+1)=obj.Kinematics.footpoint.y(idx,obj.Timing.phaseShift(jdx));
                    end
                else
                    for jdx=1:size(obj.Timing.phaseShift,1)-1
                        if jdx<size(obj.Timing.phaseShift,1)-3
                            tx(mod(jdx,6)+(mod(jdx,6)==0)*6,ceil(size(obj.Timing.phaseShift,1)/6)+floor((jdx-1)/6)+1)=obj.Kinematics.footpoint.x(idx,obj.Timing.phaseShift(jdx+3));
                            ty(mod(jdx,6)+(mod(jdx,6)==0)*6,ceil(size(obj.Timing.phaseShift,1)/6)+floor((jdx-1)/6)+1)=obj.Kinematics.footpoint.y(idx,obj.Timing.phaseShift(jdx+3));
                        end
                    end
                end
                tx(tx==0)=NaN;
                ty(ty==0)=NaN;
                for jdx=1:size(obj.Timing.phaseShift,1)/6-1
                    if idx==1                    
                        x(jdx,1:1000)=interp1(1:obj.Timing.phaseShift(jdx*6+1,1)-obj.Timing.phaseShift((jdx-1)*6+1,1),obj.Kinematics.footpoint.x(idx,obj.Timing.phaseShift((jdx-1)*6+1,1):obj.Timing.phaseShift(jdx*6+1,1)-1),linspace(1,obj.Timing.phaseShift(jdx*6+1,1)-obj.Timing.phaseShift((jdx-1)*6+1,1),1000));
                        y(jdx,1:1000)=interp1(1:obj.Timing.phaseShift(jdx*6+1,1)-obj.Timing.phaseShift((jdx-1)*6+1,1),obj.Kinematics.footpoint.y(idx,obj.Timing.phaseShift((jdx-1)*6+1,1):obj.Timing.phaseShift(jdx*6+1,1)-1),linspace(1,obj.Timing.phaseShift(jdx*6+1,1)-obj.Timing.phaseShift((jdx-1)*6+1,1),1000));
                    else
                        if jdx*6+4<size(obj.Timing.phaseShift,1)
                            x(jdx,1:1000)=interp1(1:obj.Timing.phaseShift(jdx*6+4,1)-obj.Timing.phaseShift((jdx-1)*6+4,1),obj.Kinematics.footpoint.x(idx,obj.Timing.phaseShift((jdx-1)*6+4,1):obj.Timing.phaseShift(jdx*6+4,1)-1),linspace(1,obj.Timing.phaseShift(jdx*6+4,1)-obj.Timing.phaseShift((jdx-1)*6+4,1),1000));
                            y(jdx,1:1000)=interp1(1:obj.Timing.phaseShift(jdx*6+4,1)-obj.Timing.phaseShift((jdx-1)*6+4,1),obj.Kinematics.footpoint.y(idx,obj.Timing.phaseShift((jdx-1)*6+4,1):obj.Timing.phaseShift(jdx*6+4,1)-1),linspace(1,obj.Timing.phaseShift(jdx*6+4,1)-obj.Timing.phaseShift((jdx-1)*6+4,1),1000));
                        end
                    end
                end
                ax=[ax;x];
                ay=[ay;y];
            end
            hold on
            mx=nanmean(ax(2:end-1,:));
            my=nanmean(ay(2:end-1,:));
            sy=std(ay(2:3,:));
            patch([mx mx(end:-1:1)],[my+abs(sy) my(end:-1:1)-abs(sy(end:-1:1))],0.9*ones(1,2*length(x)),'FaceColor',[0.6 0.6 0.6],'EdgeColor',[0.4 0.4 0.4])
            plot(mx,my,'k','Linewidth',2)
            %xlim([min(obj.Kinematics.footpoint.x(idx,:))-0.02 max(obj.Kinematics.footpoint.x(idx,:))+0.02])
            xlim([-0.3 0.35])
            axis equal
            ylim([-0.05 0.15])
            text(0,-0.03,'stance','color',[.35 .35 .35],'Fontsize',22)
            text(-0.235,0.02,'take-off','color',[.35 .35 .35],'Fontsize',22)
            text(0,0.1,'swing','color',[.35 .35 .35],'Fontsize',22)
            text(.3,0,'retr.','color',[.35 .35 .35],'Fontsize',22)
            xlabel('$x_{toe}$','Interpreter','Latex','Fontsize',26)
            ylabel('$y_{toe}$','Interpreter','Latex','Fontsize',26)
            set(gca,'Fontsize',16,'ytick',[0 .1])
        end
        
        function animate_ATRIAS_all(obj,mov)
            if mov
                writerObj = VideoWriter('walking.avi');
                open(writerObj);
            end
            figure(1);
            for idx=1:40:size(obj.Timing.Time,2)
                clf
                if isfinite(obj.Kinematics.position(idx,1))
                    subplot(4,4,[1 2 3 4 5 6 7 8])
                    hold on
                    drawCOM(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,2),0.1)  %draw COM
                    for jdx=1:2
                        [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                        if jdx==1
                            plot(x,y)
                        else
                            plot(x,y,'g')
                        end
                        plot([x(1) x(1)-obj.Dynamics.Fx(idx,jdx)/1000],[y(1) y(1)-obj.Dynamics.Fy(idx,jdx)/1000],'r')
                    end
                    plot(obj.Kinematics.position(:,1),obj.Kinematics.position(:,2))
                    axis equal
                    xlim([obj.Kinematics.position(idx,1)-2 obj.Kinematics.position(idx,1)+2])
                    subplot(4,4,10)
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,1))
                    hold on
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,2),'g')
                    plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-100 1000],'r--')
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    ylim([-100 1000])
                    grid on
                    title('axial leg force [N]')
                    subplot(4,4,11)
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.tanLegForce(:,1))
                    hold on
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.tanLegForce(:,2),'g')
                    plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-200 200],'r--')
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    ylim([-200 200])
                    grid on
                    title('tangential leg force [N]')
                    subplot(4,4,9)
                    plot(obj.Kinematics.position(:,1),obj.Electrics.motorCurrent(:,1),'Color',[0 0 1]);
                    hold on
                    plot(obj.Kinematics.position(:,1),obj.Electrics.motorCurrent(:,2),'Color',[0 0 0.1]);
                    grid on
                    plot([0 500],[60 60],'k--')
                    plot([0 500],[-60 -60],'k--')
                    plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-100 100],'r--')
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    ylim([-100 100])
                    legend('lA','lB','Location','NorthWest')
                    title('left leg motor currents')
                    subplot(4,4,12)
                    plot(obj.Kinematics.position(:,1),obj.Electrics.motorCurrent(:,3),'Color',[0 1 0]);
                    hold on
                    plot(obj.Kinematics.position(:,1),obj.Electrics.motorCurrent(:,4),'Color',[0 0.1 0]);
                    grid on
                    plot([0 500],[60 60],'k--')
                    plot([0 500],[-60 -60],'k--')
                    plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-100 100],'r--')
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    ylim([-100 100])
                    legend('rA','rB','Location','NorthWest')
                    title('right leg motor currents')
                    order=[13 14 15 16];
                    for mdx=1:4
                        subplot(4,4,order(mdx))
                        if mdx==1 || mdx==2
                            for ndx=2:size(obj.Timing.stanceRight,1)
                                patch([obj.Kinematics.position(obj.Timing.stanceRight(ndx-1,2),1) obj.Kinematics.position(obj.Timing.stanceRight(ndx,1),1) obj.Kinematics.position(obj.Timing.stanceRight(ndx,1),1) obj.Kinematics.position(obj.Timing.stanceRight(ndx-1,2),1)],[500 500 -500 -500],[0.9 0.9 0.9],'EdgeColor','none');
                            end
                        else
                            for ndx=1:size(obj.Timing.stanceRight,1)
                                patch([obj.Kinematics.position(obj.Timing.stanceRight(ndx,1),1) obj.Kinematics.position(obj.Timing.stanceRight(ndx,2),1) obj.Kinematics.position(obj.Timing.stanceRight(ndx,2),1) obj.Kinematics.position(obj.Timing.stanceRight(ndx,1),1)],[500 500 -500 -500],[0.9 0.9 0.9],'EdgeColor','none');
                            end
                        end
                        hold on
                        p(1)=patch([obj.Kinematics.position(:,1); obj.Kinematics.position(end,1); obj.Kinematics.position(1,1)],[obj.Kinematics.segmentVelocity(:,mdx).*(obj.Dynamics.SpringTorque(:,mdx)); 0; 0],[1 0.8 0.8],'EdgeColor','none');
                        if mdx==1 || mdx==2
                            p(2)=plot(obj.Kinematics.position(:,1),obj.Dynamics.SpringTorque(:,mdx));
                            p(3)=plot(obj.Kinematics.position(:,1),obj.Electrics.motorCurrent(:,mdx).*0.121*50,'Color',[0 0 0.1]);
                           if mdx==1
                                legend(p,'P_{motor}','\tau_{spring}','\tau_{motor}','Location','WestOutside')
                                title('torque left A')
                            else
                                title('torque left B')
                            end
                        else
                            p(2)=plot(obj.Kinematics.position(:,1),obj.Dynamics.SpringTorque(:,mdx),'Color',[0 1 0]);
                            p(3)=plot(obj.Kinematics.position(:,1),obj.Electrics.motorCurrent(:,mdx).*0.121*50,'Color',[0 0.1 0]);
                            %legend(p,'P_{motor}','\tau_{spring}','\tau_{motor}','Location','NorthWest')
                            if mdx==3
                                title('torque right A')
                            else
                                title('torque right B')
                            end
                        end
                        %text(obj.Kinematics.position(idx,1)+0.2,430,num2str(abs(obj.Dynamics.SpringTorque(idx,mdx))/abs(obj.Electrics.motorCurrent(idx,mdx).*0.121*50)));
                        plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-500 500],'r--')
                        grid on
                        xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                        ylim([-500 500])
                    end
                    drawnow
                    if mov
                        frame = getframe(gcf);
                        writeVideo(writerObj,frame);
                    end
                end
            end
            if mov
                close(writerObj);
            end
            
            
            function drawCOM(x,y,r)
                t1=linspace(0,pi/2,100);
                x1=x+[0,r*cos(t1),0];
                y1=y+[0,r*sin(t1),0];
                patch(x1,y1,'k')
                t2=linspace(pi,3*pi/2,100);
                x2=x+[0,r*cos(t2),0];
                y2=y+[0,r*sin(t2),0];
                patch(x2,y2,'k')
                t4=linspace(pi/2,pi,100);
                x4=x+[0,r*cos(t4),0];
                y4=y+[0,r*sin(t4),0];
                patch(x4,y4,'w')
                t5=linspace(3*pi/2,2*pi,100);
                x5=x+[0,r*cos(t5),0];
                y5=y+[0,r*sin(t5),0];
                patch(x5,y5,'w')
                t3=linspace(0,2*pi,400);
                x3=x+r*cos(t3);
                y3=y+r*sin(t3);
                plot(x3,y3,'k')
            end
            
            function [x,y]= animate_ATRIAS_leg(obj,idx,jdx)
                %animation of ATRIAS' leg given footpoint, hip position and leg rest length
                %returns x and y vectors to plot a leg
                %ATRIAS leg
                x(1)=obj.Kinematics.position(idx,1)+obj.Kinematics.legLength(idx,jdx)*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
                y(1)=obj.Kinematics.position(idx,2)-obj.Kinematics.legLength(idx,jdx)*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
                x(2)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
                y(2)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
                x(3)=obj.Kinematics.position(idx,1);
                y(3)=obj.Kinematics.position(idx,2);
                x(4)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
                y(4)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
                x(5)=x(1)-0.1*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx)); %connection point
                y(5)=y(1)+0.1*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
            end
        end
        
        function animate_ATRIAS(obj,mov,F)
            if mov
                writerObj(1) = VideoWriter('trajectory.avi');
                open(writerObj(1));
                if F
                    writerObj(2) = VideoWriter('axForces.avi');
                    open(writerObj(2));
                    writerObj(3) = VideoWriter('tanForces.avi');
                    open(writerObj(3));
                end
            end
            
            for idx=4000%1:3:size(obj.Timing.Time,2)
                figure(1)
                set(gcf,'units','normalized','position',[.1 .1 .7 .6])
                clf
                if isfinite(obj.Kinematics.position(idx,1))
                    subplot(4,1,[1 2 3])
                    %grounded running                    
                    %patch([8.885 9.442 9.442 8.885],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %running
                    %patch([1 1.51 1.51 1],[-.09 -.09 1 1],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %walking
                    patch([-41.72 -41.18 -41.18 -41.72],[-.09 -.09 1 1],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    hold on
                    drawCOM(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,2),0.05)  %draw COM
                    for jdx=1:2
                        [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                        if jdx==1
                            plot(x,y)
                        else
                            plot(x,y,'g')
                        end
                        %grounded running
                        %if jdx==1
                        %walking
                        %if jdx==2
                            plot([x(1) x(1)-obj.Dynamics.Fx(idx,jdx)/1000],[y(1) y(1)-obj.Dynamics.Fy(idx,jdx)/1000],'r','Linewidth',2)
                        %end
                    end
                    plot(obj.Kinematics.position(:,1),obj.Kinematics.position(:,2),'-.','Color',[0.7 0.7 0.7],'Linewidth',2)
                    axis equal
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    ylim([-0.1 1])
                    plot([-50 40],[min(min(obj.Kinematics.footpoint.y')) min(min(obj.Kinematics.footpoint.y'))],'k','Linewidth',2)
                    set(gca,'XTickLabel',[],'YTick',[0  .2 .4  .6  .8 ],'Fontsize',18)
                    ylabel('y [m]')
                    subplot(4,1,4)
                    %grounded running                    
                    %patch([8.885 9.442 9.442 8.885],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %grounded running
                    %patch([1 1.51 1.51 1],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %walking
                    patch([-41.72 -41.18 -41.18 -41.72],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    hold on
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,1),'Linewidth',2)
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,2),'g','Linewidth',2)
                    plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-100 1000],'r-.','Linewidth',2)
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    grid on
                    %grounded running
                    %ylim([-10 1000])
                    %set(gca,'Position',[0.13 0.13 .775 0.21],'YTickLabel',[0 .5 1],'XTickLabel',[0.5 1 1.5 2 2.5 3],'Fontsize',18);
                    %walking
                    ylim([-10 1000])
                    %set(gca,'Position',[0.13 0.13 .775 0.23],'YTickLabel',[0 .5 1],'XTickLabel',[0 0.5 1 1.5 2 2.5 3],'Fontsize',18);
                    %title('axial leg force [N]')
                    ylabel('F [kN]')
                    xlabel('x [m]')
                    drawnow
                    if mov
                        frame = getframe(gcf);
                        writeVideo(writerObj(1),frame);
                    end
                    if F
                        figure(3)
                        clf
                        plot(-obj.Kinematics.position(:,1),-obj.Dynamics.tanLegForce(:,1))
                        hold on
                        plot(-obj.Kinematics.position(:,1),-obj.Dynamics.tanLegForce(:,2),'g')
                        plot([-obj.Kinematics.position(idx,1) -obj.Kinematics.position(idx,1)],[-200 200],'r--')
                        xlim([-obj.Kinematics.position(idx,1)-1.5 -obj.Kinematics.position(idx,1)+1.5])
                        ylim([-200 200])
                        grid on
                        title('tangential leg force [N]')
                        %axis off
                        drawnow
                        if mov
                            frame = getframe(gcf);
                            writeVideo(writerObj(3),frame);
                        end
                    end
                end
                
            end
            %close all
            
            
            function drawCOM(x,y,r)
                t1=linspace(0,pi/2,100);
                x1=x+[0,r*cos(t1),0];
                y1=y+[0,r*sin(t1),0];
                patch(x1,y1,'w')
                t2=linspace(pi,3*pi/2,100);
                x2=x+[0,r*cos(t2),0];
                y2=y+[0,r*sin(t2),0];
                patch(x2,y2,'w')
                t4=linspace(pi/2,pi,100);
                x4=x+[0,r*cos(t4),0];
                y4=y+[0,r*sin(t4),0];
                patch(x4,y4,'m','Edgecolor','m')
                t5=linspace(3*pi/2,2*pi,100);
                x5=x+[0,r*cos(t5),0];
                y5=y+[0,r*sin(t5),0];
                patch(x5,y5,'m','Edgecolor','m')
                t3=linspace(0,2*pi,400);
                x3=x+r*cos(t3);
                y3=y+r*sin(t3);
                plot(x3,y3,'m','Linewidth',2)
            end
            
            function [x,y]= animate_ATRIAS_leg(obj,idx,jdx)
                %animation of ATRIAS' leg given footpoint, hip position and leg rest length
                %returns x and y vectors to plot a leg
                %ATRIAS leg
                x(1)=obj.Kinematics.position(idx,1)+obj.Kinematics.legLength(idx,jdx)*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
                y(1)=obj.Kinematics.position(idx,2)-obj.Kinematics.legLength(idx,jdx)*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
                x(2)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
                y(2)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
                x(3)=obj.Kinematics.position(idx,1);
                y(3)=obj.Kinematics.position(idx,2);
                x(4)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
                y(4)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
                x(5)=x(1)-0.1*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx)); %connection point
                y(5)=y(1)+0.1*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
            end
        end
        
        function obj = spring_energy(obj)
            % color scheme
            Cx = [178 223 138]./255;
            Cy = [31 120 180]./255;
            Cl = [251 154,153]./255;
            Cr = [51 160 44]./255;
            Cmo = [166 206 227]./255;
            Cro = [227 26 28]./255;
            obj = fit_SLIP(obj);
            obj = mech_energy(obj);
            %%find VLOs
            VLO = find(diff(obj.Kinematics.legAngles(:,1)>pi/2)==1);
            VLO = [VLO;find(diff(obj.Kinematics.legAngles(:,2)>pi/2)==1)];
            VLO = sort(VLO);
            obj.Timing.VLO = VLO;
            for idx=6560%1:3:size(obj.Timing.Time,2)
                fzinline = 10;
                fzout = 14;
                figure(1)
                set(gcf,'units','normalized','position',[.01 .1 .55 .8])
                clf
                if isfinite(obj.Kinematics.position(idx,1))
                    subplot(8,1,[3 4 5 6])
                    %axes('position',[0.1 0.3 0.9 0.5]);
                    %grounded running                    
                    %patch([8.885 9.442 9.442 8.885],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %running
                    %patch([1 1.51 1.51 1],[-.09 -.09 1 1],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %walking
                   %patch([-41.72 -41.18 -41.18 -41.72],[-.09 -.09 1 1],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %% draw robot
                    hold on
                    drawCOM(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,2),0.05)  %draw COM
                    for jdx=1:2
                        [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                        if jdx==1
                            %plot(x,y)
                        else
                            %plot(x,y,'g')
                        end
                        %grounded running
                        %if jdx==1
                        %walking
                        if jdx==1
                            col = Cl;
                        else
                            col = Cr;
                        end
                            plot([x(1) x(1)-obj.Dynamics.Fx(idx,jdx)/1000],[y(1) y(1)-obj.Dynamics.Fy(idx,jdx)/1000],'Color',col,'Linewidth',2)   %GRF vector
                        %end
                    end
                    plot(obj.Kinematics.position(:,1),obj.Kinematics.position(:,2),'-.','Color',[0.7 0.7 0.7],'Linewidth',2)  %CoM trajectory
                    axis equal
                    xlim([obj.Kinematics.position(idx,1)-1.7 obj.Kinematics.position(idx,1)+1.7])
                    ylim([0 1.2])
                    plot([-50 40],[min(min(obj.Kinematics.footpoint.y')) min(min(obj.Kinematics.footpoint.y'))],'k','Linewidth',2)  %ground
                    set(gca,'XTickLabel',[],'YTick',[0  .2 .4  .6  .8 1],'Fontsize',fzout)
                    ylabel('y [m]')
                    stp = find(diff(obj.Energetics.EspringLeg(:,1)>0)==1);
                    
                    %% potential energy plot
                    h = axes('position',[0.22 0.7 0.15 0.15]);
                    hold on
                    for jdx = 2:2:length(stp)-1
                        plot(h,[stp(jdx):stp(jdx+1)-1]-stp(jdx),obj.Energetics.Epot(stp(jdx):stp(jdx+1)-1)-10,'Color',[.8 .8 .8])
                        Epot(jdx-1,:) = [min(obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1)) max(obj.Energetics.Epot(stp(jdx):stp(jdx+1)-1))];
                    end
                    mean(Epot(2:end,:));
                    std(Epot(2:end,:));
                    k(1)=plot(h,0:800,obj.SLIP.E.Pot(254:end),'Linewidth',2,'Color',Cmo);
                    jdx=2;
                    k(2)=plot(h,(stp(jdx):stp(jdx+1)-1)-stp(jdx),obj.Energetics.Epot(stp(jdx):stp(jdx+1)-1)-10,'Color',Cro,'Linewidth',2);
                    xlim([0 800])
                    ylim([470 530])
                    grid on
                    box on
                    set(h,'Fontsize',fzinline);
                    set(h,'XTicklabel',[str2double(get(h,'XTicklabel')).*1e-3]','Fontsize',fzinline)
                    xlabel('t [s]','Fontsize',fzinline);
                    ylabel('E [J]','Fontsize',fzinline);
                    title('Potential Energy','Fontsize',fzinline)
                    %% kinetic energy plot
                    h = axes('position',[0.22 0.4 0.15 0.15]);
                    %plot(h,20:669,obj.SLIP.E.Kin(1:650),'Linewidth',2);
                    hold on
                    %jdx=15;
                    %plot(h,[VLO(jdx):VLO(jdx+1)-1]-VLO(jdx),obj.Energetics.Ekin(VLO(jdx):VLO(jdx+1)-1),'r')
%                     for jdx = 2:length(VLO)-1
%                         plot(h,[VLO(jdx):VLO(jdx+1)-1]-VLO(jdx),obj.Energetics.Ekin(VLO(jdx):VLO(jdx+1)-1),'Color',[.8 .8 .8])
%                         Ekin(jdx,:)=[min(obj.Energetics.Ekin(VLO(jdx):VLO(jdx+1)-1)) max(obj.Energetics.Ekin(VLO(jdx):VLO(jdx+1)-1))];
%                     end
                    for jdx = 2:2:length(stp)-1
                        plot(h,[stp(jdx):stp(jdx+1)-1]-stp(jdx),obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1),'Color',[.8 .8 .8])
                        Ekin(jdx-1,:) = [min(obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1)) max(obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1))];
                    end
                    mean(Ekin(2:end,:));
                    std(Ekin(2:end,:));
                    plot(h,0:800,obj.SLIP.E.Kin(254:end),'Linewidth',2,'Color',Cmo);
                    jdx=2;
                    plot(h,(stp(jdx):stp(jdx+1)-1)-stp(jdx),obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1),'Color',Cro,'Linewidth',2)
                    xlim([0 800])
                    ylim([0 60])
                    set(h,'Fontsize',fzinline);
                    set(h,'XTicklabel',[str2double(get(h,'XTicklabel')).*1e-3]','Fontsize',fzinline)
                    xlabel('t [s]','Fontsize',fzinline);
                    ylabel('E [J]','Fontsize',fzinline);
                    xlabh = get(h,'XLabel');
                    set(xlabh,'Position',get(xlabh,'Position') + [0 3 0])
                    box on
                    grid on
                    title('Kinetic Energy','Fontsize',fzinline)
                    %% spring energy
                    h = axes('position',[0.67 0.4 0.15 0.15]);
                    hold on
                    for jdx = 2:2:length(stp)-1
                        plot(h,[stp(jdx):stp(jdx+1)-1]-stp(jdx),sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2),'Color',[.7 .7 .7])
                        Espr(jdx-1,:) = [min(sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2)) max(sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2))];
                    end
                    mean(Espr(1:2:end,:));
                    std(Espr(1:2:end,:));
                    jdx=7;
                    k(1)=plot(-obj.SLIP.E.spring(254:1054,2),'Linewidth',2,'Color',Cmo);
                    k(2)= plot(h,[stp(jdx):stp(jdx+1)-1]-stp(jdx),sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2),'Color',Cro,'Linewidth',2);
                    xlim([0 800])
                    ylim([0 60])
                    set(h,'Fontsize',fzinline);
                    set(h,'XTicklabel',[str2double(get(h,'XTicklabel')).*1e-3]','Fontsize',fzinline)
                    box on
                    grid on
                    
                    title('Spring Energy','Fontsize',fzinline)
                    xlabel('t [s]','Fontsize',fzinline)
                    ylabel('E [J]','Fontsize',fzinline)
                    xlabh = get(h,'XLabel');
                    set(xlabh,'Position',get(xlabh,'Position') + [30 1 0])
                    %% motor energy
%                     h = axes('position',[0.67 0.7 0.15 0.15]);
%                     hold on
%                     for jdx = 2:2:length(stp)-1
%                       %[Bb,Ab]=butter(4,30/1000);
%                         plot(h,[stp(jdx):stp(jdx+1)-1]-stp(jdx),(obj.Kinematics.motors.MotorVelocity(stp(jdx):stp(jdx+1)-1,1:2)))
%                         plot(h,[stp(jdx):stp(jdx+1)-1]-stp(jdx),(obj.Kinematics.segmentVelocity(stp(jdx):stp(jdx+1)-1,1:2))


%                           obj.Energetics.Epot(stp(jdx):stp(jdx+1)-1)-10+...
%                             obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1)+...
%                             sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2),'Color',[.7 .7 .7])
%                         Espr(jdx-1,:) = [min(sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2)) max(sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2))];
%                     end
%                     plot(0:800,obj.SLIP.E.Pot(1:801)+obj.SLIP.E.Kin(1:801)-obj.SLIP.E.spring(1:801,3),'Linewidth',2)
%                     plot(obj.Energetics.Epot(stp(jdx):stp(jdx+1)-1)-10+obj.Energetics.Ekin(stp(jdx):stp(jdx+1)-1)+sum(obj.Energetics.EspringRot(stp(jdx):stp(jdx+1)-1,1:2),2),'r','Linewidth',2);
%                     xlim([0 800])
%                     ylim([500 600])
%                     grid on
%                     box on
%                     set(h,'Fontsize',fzinline);
%                     set(h,'XTicklabel',[str2num(get(h,'XTicklabel')).*1e-3]','Fontsize',fzinline)
%                     xlabel('t [s]','Fontsize',fzinline)
%                     ylabel('E [J]','Fontsize',fzinline)
%                     title('Total Energy','Fontsize',fzinline)
                    %% legend
                    h = axes('position',[0.67 0.75 0.15 0.05]);
                    plot([0 0.2],[0.2 0.2],'Color',Cmo,'Linewidth',2)
                    hold on
                    plot([0 0.2],[0.7 0.7],'Color',Cro,'Linewidth',2)
                    text(0.3,0.2,'model')
                    text(0.3,0.7,'robot')
                    xlim([0 1])
                    box off
                    axis off
                    %%
                    subplot(8,1,[7 8])
                    %axes('position',[0.1 0.05 0.9 0.2]);
                    %grounded running                    
                    %patch([8.885 9.442 9.442 8.885],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %grounded running
                    %patch([1 1.51 1.51 1],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    %walking
                    patch([-41.72 -41.18 -41.18 -41.72],[-9 -9 1000 1000],[.9 .9 .9],'EdgeColor',[0.9 .9 .9])
                    hold on
                   [Bb,Ab]=butter(4,20/1000);
                    %plot(obj.Kinematics.position(:,1),-filtfilt(Bb,Ab,obj.Dynamics.axLegForce(:,1)),'Linewidth',2)
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,1).*(-filtfilt(Bb,Ab,obj.Dynamics.axLegForce(:,1))>20).*(obj.Dynamics.axLegForce(:,1)<0),'Color',Cl,'Linewidth',2)
                    plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,2).*(-filtfilt(Bb,Ab,obj.Dynamics.axLegForce(:,2))>20).*(obj.Dynamics.axLegForce(:,2)<0),'Color',Cr,'Linewidth',2)
                    plot([obj.Kinematics.position(idx,1) obj.Kinematics.position(idx,1)],[-100 1000],'k-.','Linewidth',2)
                    xlim([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5])
                    %set(gca,'XTickLabel',[0.5 1 1.5 2 2.5 3],'Fontsize',16)
                    %grid on
                    plot([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5],[369 369],'--','Color',[.7 .7 .7]);
                    plot([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5],[500 500],'--','Color',[.7 .7 .7]);
                    plot([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5],[596 596],'--','Color',[.7 .7 .7]);
                    plot([obj.Kinematics.position(idx,1)-1.5 obj.Kinematics.position(idx,1)+1.5],[271 271],'--','Color',[.7 .7 .7]);
                    text(-15.55,271,' 2.5J','Color',[.4 .4 .4],'Fontsize',8)
                    text(-15.3,369,' 5J','Color',[.4 .4 .4],'Fontsize',8)
                    text(-15.45,500,'10J','Color',[.4 .4 .4],'Fontsize',8)
                    text(-15.1,596,'15J','Color',[.4 .4 .4],'Fontsize',8)
                    %grounded running
                    %ylim([-10 1000])
                    %set(gca,'Position',[0.13 0.13 .775 0.21],'YTickLabel',[0 .5 1],'XTickLabel',[0.5 1 1.5 2 2.5 3],'Fontsize',18);
                    %walking
                    ylim([-1 800])
                    set(gca,'XTickLabel',[0 0.5 1 1.5 2 2.5 3],'Fontsize',18);
                    %title('axial leg force [N]')
                    set(gca,'YTicklabel',[str2double(get(gca,'YTicklabel')).*1e-3]','Fontsize',fzout)
                    ylabel('F [kN]')
                    xlabel('x [m]')
                    drawnow
                    %%
                    h = axes('position',[0.55 0.27 0.3 0.03]);
                    plot([0 0.2],[0.2 0.2],'Color',Cl,'Linewidth',2)
                    hold on
                    plot([0.6 0.8],[0.2 0.2],'Color',Cr,'Linewidth',2)
                    text(0.25,0.2,'right leg')
                    text(0.85,0.2,'left leg')
                    xlim([0 1])
                    box off
                    axis off
                    %%
                    obj.Timing.steps = sort([obj.Timing.ltd;obj.Timing.rtd]);
                    for mdx = 1:length(obj.Timing.steps)-1
                        EPotInt(mdx,:) = interp1(obj.Timing.steps(mdx):(obj.Timing.steps(mdx+1)-1),obj.Energetics.Epot(obj.Timing.steps(mdx):obj.Timing.steps(mdx+1)-1),linspace(obj.Timing.steps(mdx),obj.Timing.steps(mdx+1)-1,1000));
                        EKinInt(mdx,:) = interp1(obj.Timing.steps(mdx):(obj.Timing.steps(mdx+1)-1),obj.Energetics.Ekin(obj.Timing.steps(mdx):obj.Timing.steps(mdx+1)-1),linspace(obj.Timing.steps(mdx),obj.Timing.steps(mdx+1)-1,1000));
                        ESprInt(mdx,:) = interp1(obj.Timing.steps(mdx):(obj.Timing.steps(mdx+1)-1),sum(obj.Energetics.EspringRot(obj.Timing.steps(mdx):obj.Timing.steps(mdx+1)-1,:),2),linspace(obj.Timing.steps(mdx),obj.Timing.steps(mdx+1)-1,1000));
                    end
                    obj.Energetics.EPotmean=mean(EPotInt);
                    obj.Energetics.EKinmean=mean(EKinInt);
                    obj.Energetics.ESprmean=mean(ESprInt);
%                     figure(2)
%                     %plot3(obj.Energetics.Epot,obj.Energetics.Ekin,sum(obj.Energetics.EspringRot,2),'Color',[.9 .9 .9])
%                     hold on
%                     grid on
%                     plot3(obj.Energetics.EPotmean,obj.Energetics.EKinmean,obj.Energetics.ESprmean,'r','Linewidth',2)
%                     xlabel('EPot')
%                     ylabel('EKin')
%                     zlabel('ESpring')
%                     view(45,24)
                end
            end
           
            
            function drawCOM(x,y,r)
                t1=linspace(0,pi/2,100);
                x1=x+[0,r*cos(t1),0];
                y1=y+[0,r*sin(t1),0];
                patch(x1,y1,'w')
                t2=linspace(pi,3*pi/2,100);
                x2=x+[0,r*cos(t2),0];
                y2=y+[0,r*sin(t2),0];
                patch(x2,y2,'w')
                t4=linspace(pi/2,pi,100);
                x4=x+[0,r*cos(t4),0];
                y4=y+[0,r*sin(t4),0];
                patch(x4,y4,'m','Edgecolor','m')
                t5=linspace(3*pi/2,2*pi,100);
                x5=x+[0,r*cos(t5),0];
                y5=y+[0,r*sin(t5),0];
                patch(x5,y5,'m','Edgecolor','m')
                t3=linspace(0,2*pi,400);
                x3=x+r*cos(t3);
                y3=y+r*sin(t3);
                plot(x3,y3,'m','Linewidth',2)
            end
            
            function [x,y]= animate_ATRIAS_leg(obj,idx,jdx)
                %animation of ATRIAS' leg given footpoint, hip position and leg rest length
                %returns x and y vectors to plot a leg
                %ATRIAS leg
                x(1)=obj.Kinematics.position(idx,1)+obj.Kinematics.legLength(idx,jdx)*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
                y(1)=obj.Kinematics.position(idx,2)-obj.Kinematics.legLength(idx,jdx)*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
                x(2)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
                y(2)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
                x(3)=obj.Kinematics.position(idx,1);
                y(3)=obj.Kinematics.position(idx,2);
                x(4)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
                y(4)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
                x(5)=x(1)-0.1*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx)); %connection point
                y(5)=y(1)+0.1*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
            end
        end
        
        function animate_ATRIASwTrunk(obj,range)
            clf
            try
                [Bb,Ab]=butter(3,30/1000);
                f1=filtfilt(Bb,Ab,-obj.Dynamics.tanLegForce(:,1));
                f2=filtfilt(Bb,Ab,-obj.Dynamics.tanLegForce(:,2));
            catch
                f1=-obj.Dynamics.tanLegForce(:,1);
                f2=-obj.Dynamics.tanLegForce(:,2);
            end
            f1a=-obj.Dynamics.axLegForce(:,1);
            f2a=-obj.Dynamics.axLegForce(:,2);
            %ndx1 = find(diff(f2a>0)==1);
            %ndx2 = find(diff(f2a>0)==-1);
            for idx=range
                subplot(2,2,[1 2])
                cla
                ellipse(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,2),0.5,0.1,obj.Kinematics.bodyPitch(idx),[0,0]);
                axis equal
                hold on
                for jdx=1:2
                    [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                    if jdx==1
                        plot(x,y)
                    else
                        plot(x,y,'g')
                    end
                    plot([x(1) x(1)-obj.Dynamics.Fx(idx,jdx)/300],[y(1) y(1)-obj.Dynamics.Fy(idx,jdx)/300],'r')
                end
                %CoM
                xCoM = obj.Kinematics.position(idx,1) - 0.12 * sin(3/2*pi - obj.Kinematics.bodyPitch(idx));
                yCoM = obj.Kinematics.position(idx,2) + 0.12 * cos(3/2*pi - obj.Kinematics.bodyPitch(idx));
                drawCOM(xCoM,yCoM,0.05)  %draw COM
                ylim([0 2])
                x = get(gca,'xlim');
                %title(num2str(idx))
                subplot(2,2,[3 4])
                cla
                plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,1).*(-obj.Dynamics.axLegForce(:,1)>100))
                hold on
                plot(obj.Kinematics.position(:,1),-obj.Dynamics.axLegForce(:,2).*(-obj.Dynamics.axLegForce(:,2)>100),'g')
                plot(obj.Kinematics.position(:,1),f1)
                plot(obj.Kinematics.position(:,1),f2,'g')
                xlim(x);
                
            drawnow
            end
        end
        
        function animate_ATRIASVPP(obj)                                                                                                                                                
            
            %cla
            hold on
            for idx=300:30:900
                xCoM = obj.Kinematics.position(idx,1) - 0.12 * sin(3/2*pi - obj.Kinematics.bodyPitch(idx));
                yCoM = obj.Kinematics.position(idx,2) + 0.12 * cos(3/2*pi - obj.Kinematics.bodyPitch(idx));
                gamma = -(3/2*pi - obj.Kinematics.bodyPitch(idx));
                rot = [cos(gamma), -sin(gamma);sin(gamma) cos(gamma)];
                offset=[xCoM,yCoM];
                 for jdx=1
                   [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                   % rotation
                   or = rot * ([x(1),y(1)] - offset)';
                   F = rot * ([x(1) y(1)] - offset - [obj.Dynamics.Fx(idx,jdx)/300 obj.Dynamics.Fy(idx,jdx)/300])';
                   plot([or(1) F(1)],[or(2) F(2)],'r')
                   %plot([x(1)-offset(1) x(1)-obj.Dynamics.Fx(idx,jdx)/300-offset(1)],[y(1)-offset(2) y(1)-obj.Dynamics.Fy(idx,jdx)/300-offset(2)],'r')
                 end
                title(num2str(idx))
            end               
                gamma = -(3/2*pi - obj.Kinematics.bodyPitch(idx));
                ellipse(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,2),0.5,0.1,3/2*pi,offset);
                axis equal
                hold on
                for jdx=1:2
                    [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                    if jdx==1
                        plot((x-offset(1)) .* cos(gamma) - (y - offset(2)) .* sin(gamma),(x - offset(1)) .* sin(gamma) + (y - offset(2)) .* cos(gamma))
                    else
                        plot((x-offset(1)) .* cos(gamma) - (y - offset(2)) .* sin(gamma),(x - offset(1)) .* sin(gamma) + (y - offset(2)) .* cos(gamma),'g')
                    end
                    %plot([x(1)-offset(1) x(1)-obj.Dynamics.Fx(idx,jdx)/300-offset(1)],[y(1)-offset(2) y(1)-obj.Dynamics.Fy(idx,jdx)/300-offset(2)],'r')
                end
                %CoM
                drawCOM(xCoM-offset(1),yCoM-offset(2),0.05)  %draw COM
                title(num2str(obj.Kinematics.bodyPitch(idx)*180/pi))
            drawnow
                           
        end
        
        function DW2014_plot(obj)
            clf
            subplot(1,4,1)
            animate_ATRIASwTrunk(obj,300)
            box off
            axis equal
            axis off
            ylim([0 2])
            title('early stance')
            subplot(1,4,2)
            animate_ATRIASwTrunk(obj,540)
            box off
            axis off
            axis equal
            ylim([0 2])
            title('single support')
            subplot(1,4,3)
            animate_ATRIASwTrunk(obj,830)
            box off
            axis off
            axis equal
            ylim([0 2])
            title('late stance')
            subplot(1,4,4)
            animate_ATRIASVPP(obj)
            ylim([-1 1])
            box off
            axis off
            axis equal
            title('stance phase')
            %get(gca,'ylim')
        end
  
        
        function obj=meanForce(obj)
            for idx=2:size(obj.Timing.stanceRight,1)-1
                start=find(obj.Dynamics.axLegForce(1:obj.Timing.stanceRight(idx,1),2)>0,1,'last');
                fini=obj.Timing.stanceRight(idx,2)+find(obj.Dynamics.axLegForce(obj.Timing.stanceRight(idx,2):end,2)>0,1,'first');
                obj.Dynamics.avg.Fr(idx-1,1:fini-start+1)=obj.Dynamics.axLegForce(start:fini,2);
                obj.Dynamics.avg.sDefl(idx,3:4)=max(abs(obj.Kinematics.springDeflection(start:fini,3:4)));
                start=find(obj.Dynamics.axLegForce(1:obj.Timing.stanceRight(idx,2),1)>0,1,'last');
                fini=obj.Timing.stanceRight(idx+1,1)+find(obj.Dynamics.axLegForce(obj.Timing.stanceRight(idx+1,1):end,1)>0,1,'first');
                obj.Dynamics.avg.Fl(idx-1,1:fini-start+1)=obj.Dynamics.axLegForce(start:fini,1);
                obj.Dynamics.avg.sDefl(idx,1:2)=max(abs(obj.Kinematics.springDeflection(start:fini,1:2)));
            end
            obj.Dynamics.avg.mFl=mean(obj.Dynamics.avg.Fl);
            obj.Dynamics.avg.mFr=mean(obj.Dynamics.avg.Fr);
            obj.Dynamics.avg.stdFl=std(obj.Dynamics.avg.Fl);
            obj.Dynamics.avg.stdFr=std(obj.Dynamics.avg.Fr);
            patch([1:length(obj.Dynamics.avg.mFl) length(obj.Dynamics.avg.mFl):-1:1],[-obj.Dynamics.avg.mFl-obj.Dynamics.avg.stdFl -obj.Dynamics.avg.mFl(end:-1:1)+obj.Dynamics.avg.stdFl(end:-1:1)],[0.9 0.9 0.9])
            hold on
            plot(-obj.Dynamics.avg.mFl)
            figure(2)
            patch([1:length(obj.Dynamics.avg.mFr) length(obj.Dynamics.avg.mFr):-1:1],[-obj.Dynamics.avg.mFr-obj.Dynamics.avg.stdFr -obj.Dynamics.avg.mFr(end:-1:1)+obj.Dynamics.avg.stdFr(end:-1:1)],[0.9 0.9 0.9])
            hold on
            plot(-obj.Dynamics.avg.mFr)
            %% energy
            obj.Dynamics.avg.E=obj.robotProperties.m*obj.robotProperties.g*obj.Kinematics.position(:,2)+ ...
                1/2*obj.robotProperties.m*sqrt(sum(obj.Kinematics.velocity.^2,2))+...
                sum(1/2*1400.*abs(obj.Kinematics.springDeflection),2);
        end
        
        function obj=returnmap(obj)
            %find apices
            for idx=1:size(obj.Timing.stanceRight,1)-1
                [~, yr]=max(obj.Kinematics.position(obj.Timing.stanceRight(idx,1):obj.Timing.stanceRight(idx,2),2));
                [~, yl]=max(obj.Kinematics.position(obj.Timing.stanceRight(idx,2):obj.Timing.stanceRight(idx+1,1),2));
                obj.Analytics.apices(idx,:)=[yl+obj.Timing.stanceRight(idx,2) yr+obj.Timing.stanceRight(idx,1)];
            end
            obj.Kinematics.positionSmooth = SPoleFilt(obj.Kinematics.position, exp(-2*pi*obj.Analytics.FrequencyCutOff / obj.Analytics.SamplingRate), 2,obj.Analytics.type);
            obj.Kinematics.velocitySmooth = SPoleFilt(obj.Kinematics.velocity, exp(-2*pi*obj.Analytics.FrequencyCutOff / obj.Analytics.SamplingRate), 2,obj.Analytics.type);
        end
        
        function animate_force_sequence(obj,mov,F)
            if mov
                writerObj(1) = VideoWriter('trajectory.avi');
                open(writerObj(1));
                writerObj(2) = VideoWriter('axForces.avi');
                open(writerObj(2));
                writerObj(3) = VideoWriter('tanForces.avi');
                open(writerObj(3));
            end
            clf
            for idx=1330:150:1800
                figure(1)
                if isfinite(obj.Kinematics.position(idx,1))
                    %subplot(3,1,[1 2])
                    hold on
                    drawCOM(-obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,2),0.03)  %draw COM
                    for jdx=1:2
                        [x,y]=animate_ATRIAS_leg(obj,idx,jdx);
                        if jdx==1
                            plot(-x,y,'Color',[.4 .4 .4])
                            plot([-x(1) -x(1)+obj.Dynamics.Fx(idx,jdx)/1000],[y(1) y(1)-obj.Dynamics.Fy(idx,jdx)/1000],'r')
                        else
                            plot(-x,y,'Color',[.9 .9 .9])
                        end
                        end
                    plot(-obj.Kinematics.position(:,1),obj.Kinematics.position(:,2))
                    axis equal
                    xlim([-obj.Kinematics.position(idx,1)-.5 -obj.Kinematics.position(idx,1)+.8])
                    ylim([-0.1 1])
                    plot([-4 0],[0 0],'k','Linewidth',2)
                    %title('axial leg force [N]')
                    drawnow
                    if mov
                        frame = getframe(gcf);
                        writeVideo(writerObj(1),frame);
                    end
                    if F
                        figure(3)
                        clf
                        plot(-obj.Kinematics.position(:,1),-obj.Dynamics.tanLegForce(:,1))
                        hold on
                        plot(-obj.Kinematics.position(:,1),-obj.Dynamics.tanLegForce(:,2),'g')
                        plot([-obj.Kinematics.position(idx,1) -obj.Kinematics.position(idx,1)],[-200 200],'r--')
                        xlim([-obj.Kinematics.position(idx,1)-1.5 -obj.Kinematics.position(idx,1)+1.5])
                        ylim([-200 200])
                        grid on
                        title('tangential leg force [N]')
                        %axis off
                        drawnow
                        if mov
                            frame = getframe(gcf);
                            writeVideo(writerObj(3),frame);
                        end
                    end
                end
                
            end
            %close all
            
            
            %function drawCOM(x,y,r)
%                 t1=linspace(0,pi/2,100);
%                 x1=x+[0,r*cos(t1),0];
%                 y1=y+[0,r*sin(t1),0];
%                 patch(x1,y1,'k')
%                 t2=linspace(pi,3*pi/2,100);
%                 x2=x+[0,r*cos(t2),0];
%                 y2=y+[0,r*sin(t2),0];
%                 patch(x2,y2,'k')
%                 t4=linspace(pi/2,pi,100);
%                 x4=x+[0,r*cos(t4),0];
%                 y4=y+[0,r*sin(t4),0];
%                 patch(x4,y4,'w')
%                 t5=linspace(3*pi/2,2*pi,100);
%                 x5=x+[0,r*cos(t5),0];
%                 y5=y+[0,r*sin(t5),0];
%                 patch(x5,y5,'w')
%                 t3=linspace(0,2*pi,400);
%                 x3=x+r*cos(t3);
%                 y3=y+r*sin(t3);
%                 plot(x3,y3,'k')
%             end
            
%             %function [x,y]= animate_ATRIAS_leg(obj,idx,jdx)
%                 %animation of ATRIAS' leg given footpoint, hip position and leg rest length
%                 %returns x and y vectors to plot a leg
%                 %ATRIAS leg
%                 x(1)=obj.Kinematics.position(idx,1)+obj.Kinematics.legLength(idx,jdx)*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
%                 y(1)=obj.Kinematics.position(idx,2)-obj.Kinematics.legLength(idx,jdx)*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
%                 x(2)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
%                 y(2)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
%                 x(3)=obj.Kinematics.position(idx,1);
%                 y(3)=obj.Kinematics.position(idx,2);
%                 x(4)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
%                 y(4)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
%                 x(5)=x(1)-0.1*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx)); %connection point
%                 y(5)=y(1)+0.1*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
%             end
        end
        
        function obj = force_angle(obj)
            Cro = [227 26 28]./255;
            figure(1);
            set(gcf,'position',[81 181 1070 360])
            % Find touchdown and takeoff
            [Bb,Ab]=butter(2,obj.Analytics.FrequencyCutOff/obj.Analytics.SamplingRate);
            F = filtfilt(Bb,Ab,-obj.Dynamics.axLegForce);
            F(F<85)=0; %filter flight forces
            stance=F~=0;        %identify stance phases
            transs=[0 0;diff(stance)];              %identify transistions
            td(:,1)=find(transs(:,1)==1);           %find touchdown indices
            %td(:,2)=find(transs(:,2)==1);
            to(:,1)=find(transs(:,1)==-1);           %find take-off indices
            %to(:,2)=find(transs(:,2)==-1,length(to));
            obj.Timing.stanceTime = [];
            % Normalize time by the step length
            Fax=[];
            for idx=1%1:2     %l/r
                if to(1,idx)>td(1,idx)
                    for jdx=1:size(to,1)
                        Ftan(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx,idx)+3,[0; obj.Dynamics.tanLegForce(td(jdx,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx,idx)+3,1000))';
                        Fx(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx,idx)+3,[0; obj.Dynamics.Fx(td(jdx,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx,idx)+3,1000))';
                        Fy(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx,idx)+3,[0; obj.Dynamics.Fy(td(jdx,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx,idx)+3,1000))';
                        ll(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx,idx)+1,obj.Kinematics.legAngles(td(jdx,idx):to(jdx,idx),idx),linspace(1,to(jdx,idx)-td(jdx,idx)+3,1000))';
                        Fax(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx,idx)+3,[0; obj.Dynamics.axLegForce(td(jdx,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx,idx)+3,1000))';
                        obj.Timing.stanceTime = [obj.Timing.stanceTime;to(jdx,idx)-td(jdx,idx)+3];
                    end
                else
                    for jdx=2:size(to,1)
                        Ftan(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx-1,idx)+3,[0; obj.Dynamics.tanLegForce(td(jdx-1,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx-1,idx)+3,1000))';
                        Fx(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx-1,idx)+3,[0; obj.Dynamics.Fx(td(jdx-1,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx-1,idx)+3,1000))';
                        Fy(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx-1,idx)+3,[0; obj.Dynamics.Fy(td(jdx-1,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx-1,idx)+3,1000))';
                        ll(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx-1,idx)+1,obj.Kinematics.legAngles(td(jdx-1,idx):to(jdx,idx),idx),linspace(1,to(jdx,idx)-td(jdx-1,idx)+1,1000))';
                        Fax(:,size(Fax,2)+1)=interp1(1:to(jdx,idx)-td(jdx-1,idx)+3,[0; obj.Dynamics.axLegForce(td(jdx-1,idx):to(jdx,idx),idx); 0],linspace(1,to(jdx,idx)-td(jdx-1,idx)+3,1000))';
                        obj.Timing.stanceTime = [obj.Timing.stanceTime;to(jdx,idx)-td(jdx-1,idx)+3];
                    end
                end
            end
            % Find means and standard deviations
            Fa=[nanmean(Fax,2) nanstd(Fax')'];
            Ft=[nanmean(Ftan,2) nanstd(Ftan')'];
            Fxm=[nanmean(Fx,2) nanstd(Fx')'];
            Fym=[nanmean(Fy,2) nanstd(Fy')'];
            l=[nanmean(ll,2) nanstd(ll')'];
            obj.SLIP.statistics.Fxm=nanmean(Fx,2);
            obj.SLIP.statistics.Fym=nanmean(Fy,2);

            % Angle plot
            clf
            axes('Position',[0.68 0.15 0.3 0.8]); hold on; grid on;
            patch([0 1000 1000 0],[-atand(0.2) -atand(0.2) atand(0.2) atand(0.2)],[.9 .9 .9],'EdgeColor','none');
            patch([0 1000 1000 0],[-atand(0.1) -atand(0.1) atand(0.1) atand(0.1)],[.8 .8 .8],'EdgeColor','none');
            % TODO: Figure out how to remove this patch without changing other figure properties
            patch([0 996 996 0],[-10 -10 10 10],[0.9 .9 .9],'Edgecolor','none','Facealpha',0.0)
            dphi=atan(Ftan./Fax)*180/pi;   % Force angle devation from axial
            patch([2:996 996:-1:2],[-dphi(2:996,1)+abs(dphi(2:996,2));-dphi(996:-1:2,1)-abs(dphi(996:-1:2,2))],[0.7 0.7 0.7],'FaceAlpha',0)
            plot(-dphi(:,1),'Linewidth',2,'Color',Cro)
            ylim([-45 45])
            rectangle('Position',[600,-39,350,20],'FaceColor','w');
            plot([620 700],[-23 -23],'Color',Cro,'Linewidth',2);
            text(710,-23,'$\Delta \varphi$','Interpreter','Latex','Fontsize',14);
            patch([620 620 700 700],[-30 -28 -28 -30],[.8 .8 .8],'Edgecolor','none');
            text(710,-29,'$\leq 0.1F_{ax}$','Interpreter','Latex','Fontsize',14)
            patch([620 620 700 700],[-36 -34 -34 -36],[.9 .9 .9],'Edgecolor','none');
            text(710,-35,'$\leq 0.2F_{ax}$','Interpreter','Latex','Fontsize',14)
            set(gca,'Fontsize',16)
            set(gca,'XTicklabel',[str2num(get(gca,'XTicklabel')).*1e-1]','Fontsize',16)
            xlabel('t [% stance time]','Fontsize',16)
            ylabel('$\Delta\varphi [^o]$','Fontsize',16,'Interpreter','Latex')

            % ATRIAS diagram
            frame = 1150;
            axes('Position',[0.40 0.11 0.21 0.81])
            [x,y]=animate_ATRIAS_leg(obj,frame,1);
            plot(x,y,'k','Linewidth',2)
            hold on
            text(x(1)-0.175,y(1)+0.55,'leg axis','Fontsize',11,'Color','k','FontAngle','italic')
            plot(x([1 3]),y([1 3]),'k--')
            plot([x(3) x(3)+0.2],[y(3) y(3)],'k--','Linewidth',3)
            t=0:0.01:obj.Kinematics.legAngles(frame,1);
            xr=0.15*cos(t);
            yr=-0.15*sin(t);
            plot(x(3)+xr,y(3)+yr,'k-.')
            text(x(3)+xr(1),y(3)+yr(40),'$\varphi_l$','Interpreter','Latex','Fontsize',16)
            ang = pi/3;
            plot([x(1) x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)],...
                [y(1) y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)],'Color',Cro)
            %arrowhead
            plot([x(1)-obj.Kinematics.legLength(frame,1)*cos(ang) x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)+0.01 x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)+0.035 x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)],...
                [y(1)+obj.Kinematics.legLength(frame,1)*sin(ang) y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)-0.05 y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)-0.03 y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)],'Color',Cro)
            patch([x(1)-obj.Kinematics.legLength(frame,1)*cos(ang) x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)+0.01 x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)+0.035 x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)],...
                [y(1)+obj.Kinematics.legLength(frame,1)*sin(ang) y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)-0.05 y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)-0.03 y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)],Cro,'Edgecolor',Cro)
            text(x(1)-obj.Kinematics.legLength(frame,1)*cos(ang)+0.01,y(1)+obj.Kinematics.legLength(frame,1)*sin(ang)+0.02,'$F$','Interpreter','Latex','Fontsize',16)
            plot([x(1) x(1)-.2],[y(1) y(1)],'k--','Linewidth',2)
            t=0:0.01:ang;
            xr=x(1)-0.15*cos(t);
            yr=y(1)+0.15*sin(t);
            plot(xr,yr,'k-.')
            text(x(1)-0.3,y(1)+0.06,'$\varphi_F$','Interpreter','Latex','Fontsize',16)
            patch([x(1)+0.2 x(1)+0.15 x(1)+0.15 x(1)+0.2],[y(1) y(1)+0.02 y(1)-0.02 y(1)],[178 223 138]./255,'Edgecolor',[178 223 138]./255)
            plot([x(1) x(1)+0.2 x(1)+0.15 x(1)+0.15 x(1)+0.2],[y(1) y(1) y(1)+0.02 y(1)-0.02 y(1)],'Color',[178 223 138]./255,'Linewidth',2)
            text(x(1)+0.22,y(1),'$F_x$','Interpreter','Latex','Fontsize',16)
            patch([x(1) x(1)-0.02 x(1)+0.02 x(1)],[y(1) y(1)-0.05 y(1)-0.05 y(1)],[31 120 180]./255,'Edgecolor',[31 120 180]./255)
            plot([x(1) x(1)],[y(1) y(1)-0.2],'Linewidth',2,'Color',[31 120 180]./255)
            text(x(1)+0.02,y(1)-0.1,'$F_y$','Interpreter','Latex','Fontsize',16)
            axis off
            axis equal

            % Force plot
            axes('Position',[0.08 0.15 0.3 0.8]); hold on; grid on
            patch([obj.SLIP.tnorm; 0],[obj.SLIP.Fy; 0],[0.8 0.8 0.8],'Edgecolor','none')
            patch([obj.SLIP.tnorm; 0],[obj.SLIP.Fx; 0],[0.7 0.7 0.7],'Edgecolor','none')
            patch([1:1000 1000:-1:1],[-Fym(:,1)+abs(Fym(:,2)); -Fym(end:-1:1,1)-abs(Fym(end:-1:1,2))],[0.9 0.9 0.9],'Facecolor','none')
            p(2)=plot(-Fym(:,1),'Linewidth',2,'Color',[31 120 180]./255);
            patch([1:1000 1000:-1:1],[-Fxm(:,1)+abs(Fxm(:,2)); -Fxm(end:-1:1,1)-abs(Fxm(end:-1:1,2))],[0.9 0.9 0.9])
            p(1)=plot(-Fxm(:,1),'Linewidth',2,'Color',[178 223 138]./255);
            legend(p,'F_x','F_y','Location','North')
            set(gca,'Fontsize',16)
            set(gca,'XTickLabel',get(gca,'XTick')./10)
            %set(gca,'XTickLabel',[0 50 100],'YTickLabel',[-.4 -.2 0 .2 .4 .6 .8],'Fontsize',16)
            %plot(obj.SLIP.tnorm,obj.SLIP.Fy,'r','Linewidth',2);
            %plot(obj.SLIP.tnorm,obj.SLIP.Fx,'r','Linewidth',2);
            xlim([0 1000]);
            xlabel('t [% stance time]','Fontsize',16)
            ylabel('F [N]','Fontsize',16)
            text(-240,800,'A','Fontsize',24,'Fontweight','Bold')
            text(1020,800,'B','Fontsize',24,'Fontweight','Bold')
            text(1800,800,'C','Fontsize',24,'Fontweight','Bold')

            
            
            function [x,y]= animate_ATRIAS_leg(obj,idx,jdx)
                %animation of ATRIAS' leg given footpoint, hip position and leg rest length
                %returns x and y vectors to plot a leg
                %ATRIAS leg
                x(1)=obj.Kinematics.position(idx,1)+obj.Kinematics.legLength(idx,jdx)*cos(obj.Kinematics.legAngles(idx,jdx));
                y(1)=obj.Kinematics.position(idx,2)-obj.Kinematics.legLength(idx,jdx)*sin(obj.Kinematics.legAngles(idx,jdx));
                x(2)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+obj.Kinematics.Beta(idx,jdx));
                y(2)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+obj.Kinematics.Beta(idx,jdx));
                x(3)=obj.Kinematics.position(idx,1);
                y(3)=obj.Kinematics.position(idx,2);
                x(4)=obj.Kinematics.position(idx,1)+0.8*obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)-obj.Kinematics.Beta(idx,jdx));
                y(4)=obj.Kinematics.position(idx,2)-0.8*obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)-obj.Kinematics.Beta(idx,jdx));
                x(5)=x(1)-0.1*cos(obj.Kinematics.legAngles(idx,jdx)-obj.Kinematics.Beta(idx,jdx)); %connection point
                y(5)=y(1)+0.1*sin(obj.Kinematics.legAngles(idx,jdx)-obj.Kinematics.Beta(idx,jdx));
            end
        end
        
        function obj=hop_phasing(obj)
           try
               [Bb,Ab]=butter(5,(obj.Analytics.FrequencyCutOff+20)/obj.Analytics.SamplingRate);
               [Ba,Aa]=butter(2,(obj.Analytics.FrequencyCutOff)/obj.Analytics.SamplingRate);
               pos = obj.Kinematics.position(isfinite(obj.Kinematics.position(:,2)),2);
               toe = obj.Kinematics.footpoint.y(2,isfinite(obj.Kinematics.footpoint.y(2,:)))';
               del1 = filtfilt(Bb,Ab,pos-toe);
               del2 = filtfilt(Ba,Aa,pos-toe);
           end
           td = find(diff(del1 < 0.845)==1 & diff(del2) < 0 & toe(2:end) < 0.03);
           for idx = 1:length(td)
               td(idx) = find(obj.Dynamics.axLegForce(1:td(idx)+80,obj.Analytics.leg)>-35,1,'last');
           end
           for idx=1:min([length(td) length(obj.Timing.stanceRight)])
               if ~isempty(find(obj.Dynamics.axLegForce(td(idx)+80:end,obj.Analytics.leg)>0,1))
                   to(idx)=find(obj.Dynamics.axLegForce(td(idx)+80:end,obj.Analytics.leg)>0,1)+td(idx)+79;
               else
                   to(idx)=NaN;
               end
           end
           if td(1)>to(1)
               td=td(2:end);
           end
           td1=find(td(:,1)>to(1),1);
           td = td(td1-1:end);
           l = min([length(td) length(to)]);
           obj.Timing.stanceHop = [td(1:l) to(1:l)'];
        end
        
        function obj = hopmeanforce(obj)
            stancelength=obj.Timing.stanceHop(:,2)-obj.Timing.stanceHop(:,1);
            meanstancelength=nanmedian(stancelength);
            obj.Dynamics.analysis.tstancemean=meanstancelength;
            validhops = abs(stancelength-meanstancelength) < 0.1*meanstancelength; %hops of duration within 10% of median hop length
            obj.Dynamics.analysis.validhops = validhops;
            for idx=find(validhops==1)'
                eqforce(idx,:) = interp1(0:stancelength(idx),obj.Dynamics.axLegForce(obj.Timing.stanceHop(idx,1):obj.Timing.stanceHop(idx,2),obj.Analytics.leg),linspace(0,stancelength(idx),1000));
            end
            obj.Dynamics.analysis.Fresampled = eqforce';
            obj.Dynamics.analysis.Fmean=mean(eqforce)';
            obj.Dynamics.analysis.Fstd=std(eqforce)';
        end
        
        function obj = matchforces(obj,nl)
            %% find touch-down speed and height
            obj = obj.hop_phasing;
            obj = hopmeanforce(obj);
            vmean = mean(obj.Kinematics.velocity(obj.Timing.stanceHop(:,1),2));
            hmean = mean(obj.Kinematics.position(obj.Timing.stanceHop(:,1),2));
            addpath C:\Users\Work\Documents\DRL\MyRepos\spring-mass-model
            S=nlSLIP;
            S.intprops.nl=nl;
            S.params.k=24246;
            S.state.IC=[0 0 hmean vmean];
            S.state.xe=S.state.IC;
            S.params.l0=hmean;
            if S.intprops.nl==1
                [krot]=fminsearch(@(k) modelforce(obj,S,k),4118);
                obj.Dynamics.analysis.kmatch = krot;
            else
                [k]=fminsearch(@(k) modelforce(obj,S,k),27000);
                obj.Dynamics.analysis.kmatch = k;
            end
            obj.Dynamics.analysis.vTD = vmean;
            obj.Dynamics.analysis.hTD = hmean;
        end
        
        function obj = plotSLIPatrias(obj,nl)
            clf
            hold on
            for idx=1:length(obj.Timing.stanceHop)
                if obj.Dynamics.analysis.validhops(idx)==1
                    plot([0:obj.Timing.stanceHop(idx,2)-obj.Timing.stanceHop(idx,1)]./1000,-obj.Dynamics.axLegForce(obj.Timing.stanceHop(idx,1):obj.Timing.stanceHop(idx,2),obj.Analytics.leg),'Color',[0.8 0.8 0.8]);
                end
            end
            h(1)=plot(linspace(0,obj.Dynamics.analysis.tstancemean/1000,1000),-obj.Dynamics.analysis.Fmean,'b','Linewidth',2);
            addpath C:\Users\Work\Documents\DRL\MyRepos\spring-mass-model
            S=nlSLIP;
            S.intprops.nl=nl;
            S.state.IC = [0 0 obj.Dynamics.analysis.hTD obj.Dynamics.analysis.vTD];
            S.state.xe = S.state.IC;
            S.params.l0 = obj.Dynamics.analysis.hTD;
            S1 = S;
            S1 = SLIPHOP(S1);
            obj.Dynamics.analysis.cmodel.F = S1.data.F;
            obj.Dynamics.analysis.cmodel.l = S1.params.l0 - S1.state.x(:,3);
            obj.Dynamics.analysis.cmodel.t = S1.state.t;
            h(2)=plot(S1.state.t,S1.data.F,'m');
            hold on
            S2 = S;
            if S2.intprops.nl
                S2.params.krot = obj.Dynamics.analysis.kmatch;
            else
                S2.params.k = obj.Dynamics.analysis.kmatch;
            end
            S2 = SLIPHOP(S2);
            obj.Dynamics.analysis.mmodel.F = S2.data.F;
            obj.Dynamics.analysis.mmodel.l = S2.params.l0 - S2.state.x(:,3);
            obj.Dynamics.analysis.mmodel.t = S2.state.t;
            h(3)=plot(S2.state.t,S2.data.F,'r','Linewidth',2);
            grid on
            if nl
                legend(h,'mean robot force','desired force profile (k = 4118Nm/rad)',['matched force profile (k = ',num2str(round(obj.Dynamics.analysis.kmatch)),'Nm/rad)'],'Location','South')
            else
                legend(h,'mean robot force','desired force profile (k = 24246N/m)',['matched force profile (k = ',num2str(round(obj.Dynamics.analysis.kmatch)),'N/m)'],'Location','South')
            end
        end
        
        function obj = force_length(obj)
            clf
            hold on
            for idx = 1:length(obj.Timing.stanceHop)
                if obj.Dynamics.analysis.validhops(idx)
                    plot(obj.Kinematics.legLength(obj.Timing.stanceHop(idx,1),obj.Analytics.leg)-obj.Kinematics.legLength(obj.Timing.stanceHop(idx,1):obj.Timing.stanceHop(idx,2),obj.Analytics.leg),...
                        -obj.Dynamics.axLegForce(obj.Timing.stanceHop(idx,1):obj.Timing.stanceHop(idx,2),obj.Analytics.leg),'Color',[.8 .8 .8])
                end
            end
            plot(obj.Dynamics.analysis.cmodel.l,obj.Dynamics.analysis.cmodel.F,'m')
            plot(obj.Dynamics.analysis.mmodel.l,obj.Dynamics.analysis.mmodel.F,'r')
        end
        
        function obj = motor_position_length(obj)
            hold
            for idx = 1:length(obj.Timing.stanceHop)
                if obj.Dynamics.analysis.validhops(idx)
                    plot(obj.Kinematics.legLength(obj.Timing.stanceHop(idx,1),obj.Analytics.leg)-obj.Kinematics.legLength(obj.Timing.stanceHop(idx,1):obj.Timing.stanceHop(idx,2),obj.Analytics.leg),...
                        obj.Electrics.motorCurrent(obj.Timing.stanceHop(idx,1):obj.Timing.stanceHop(idx,2),3:4),'Color',[.8 .8 .8])
                end
            end
        end
        
        
        
    end
    
    methods (Access = private)
        function delta = modelforce(obj,S,k)
            if S.intprops.nl
                S.params.krot=k;
            else
                S.params.k=k;
            end
            S = int_hop_stance(S);
            S = hop_force(S);
            ST = interp1(S.state.t,S.data.F,linspace(0,S.state.t(end),1000));
            clf
            plot(ST)
            hold on
            plot(-obj.Dynamics.analysis.Fmean,'r')
            if S.intprops.nl
                title(num2str(S.params.krot));
            else
                title(num2str(S.params.k));
            end
            xlabel([num2str(obj.Dynamics.analysis.tstancemean/1000) num2str(S.state.t(end))])
            drawnow
            delta=abs(S.state.t(end)-obj.Dynamics.analysis.tstancemean/1000);
            %delta = sum((ST'+obj.Dynamics.analysis.Fmean).^2);
        end
        
        
        
        
        
    end
end

function [] = ellipse(xc,yc,x,y,phi,offset)
% input ellipse parameters
theta_grid = linspace(0,2*pi,500);
X0=xc;
Y0=yc;
a=x;
b=y;

% the ellipse in x and y coordinates
ellipse_x_r  = 0.417 + a*cos( theta_grid );
ellipse_y_r  = b*sin( theta_grid );

%Define a rotation matrix
R = [ cos(phi) sin(phi); -sin(phi) cos(phi) ];

%let's rotate the ellipse to some angle phii
r_ellipse = R * [ellipse_x_r;ellipse_y_r];
plot(xc+r_ellipse(1,:)-offset(1),yc+r_ellipse(2,:)-offset(2),'-')
end

function drawCOM(x,y,r)
t1=linspace(0,pi/2,100);
x1=x+[0,r*cos(t1),0];
y1=y+[0,r*sin(t1),0];
patch(x1,y1,'w')
t2=linspace(pi,3*pi/2,100);
x2=x+[0,r*cos(t2),0];
y2=y+[0,r*sin(t2),0];
patch(x2,y2,'w')
t4=linspace(pi/2,pi,100);
x4=x+[0,r*cos(t4),0];
y4=y+[0,r*sin(t4),0];
patch(x4,y4,'k','Edgecolor','k')
t5=linspace(3*pi/2,2*pi,100);
x5=x+[0,r*cos(t5),0];
y5=y+[0,r*sin(t5),0];
patch(x5,y5,'k','Edgecolor','k')
t3=linspace(0,2*pi,400);
x3=x+r*cos(t3);
y3=y+r*sin(t3);
plot(x3,y3,'k','Linewidth',2)
end

function [x,y]= animate_ATRIAS_leg(obj,idx,jdx)
%animation of ATRIAS' leg given footpoint, hip position and leg rest length
%returns x and y vectors to plot a leg
%ATRIAS leg
x(1)=obj.Kinematics.position(idx,1)+obj.Kinematics.legLength(idx,jdx)*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));                    %toe
y(1)=obj.Kinematics.position(idx,2)-obj.Kinematics.legLength(idx,jdx)*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2));
x(2)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));   %back knee
y(2)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)+obj.Kinematics.Beta(idx,jdx));
x(3)=obj.Kinematics.position(idx,1);                                                                                                                                        %hip
y(3)=obj.Kinematics.position(idx,2);
x(4)=obj.Kinematics.position(idx,1)+obj.robotProperties.s*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));   %front knee
y(4)=obj.Kinematics.position(idx,2)-obj.robotProperties.s*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
x(5)=x(1)-0.1*cos(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));                                               %connection point
y(5)=y(1)+0.1*sin(obj.Kinematics.legAngles(idx,jdx)+(obj.Kinematics.bodyPitch(idx,jdx)-3*pi/2)-obj.Kinematics.Beta(idx,jdx));
end


